#!/usr/bin/env python3
import rospy,math,os
from geometry_msgs.msg import PoseStamped, TwistStamped,Vector3,Twist
import tf.transformations as tf
from std_msgs.msg import Header, String, Int32
from sensor_msgs.msg import NavSatFix
import sys, math, time, openpyxl
import numpy as np


class L1:
    def __init__(self):
        
        """
        init
        """
        self.vehicle_type = 'uav'
        self.id = uav_id    #注意id号为1，输入
        # print(self.id)
        
        self.g = 9.81
        self.q_2, self.dq_2 = 0.0, 0.0
        self.r_2, self.dr_2 = 0.0, 0.0
        self.q_lon3, self.q_lat3 = 0.0, 0.0
        self.dq_lon3, self.dq_lat3 = 0.0, 0.0

        #制导指令初始化
        self.v = 0.0
        self.v_sp = speed
        self.roll = 0.0
        self.alt = alt[uav_id-1]
        self.roll_thres = 36  # 海上XFD，微微增大一些
        # self.roll_thres = 18  # 郑州泡沫机
        
        self.flag = 0
        self.flag_cmd = 0 #cmd_stage标志位
        self.scene = 0
        self.um = 0.0     #二维输出
        self.dpsi = 0.0
        
        #集结
        # self.vs = 142.0     # 修改集结速度，一圈时间54.63
        self.print_h = 1.0
        self.d_m = 270.0
        self.t_last_loop = 0.0      #初始时间
        self.t_now_loop = 0.0    #目前时间
        
        self.gps_received = False

        self.num = 5 # 无人机数量
        self.uav_pos = [0,0,0]
        self.uav_vel = [0,0,0]
        self.uav_angle = [0,0,0]
        # self.v = 0
        # self.global_poses = [NavSatFix() for i in range(self.num)]
        # self.local_poses_gps = [PoseStamped() for i in range(self.num)]
        #communication_ids = np.argwhere(self.A[self.id, :] == 1)#找出为1的元素索引id，为二维数组
        #self.communication_ids = communication_ids.flatten().tolist()#转为一维数组并转换为列表，与本机有通信的id(如果id从0开始)
        self.begin_lat = 0.0
        self.begin_lon = 0.0
        self.begin_alt = 0.0

        self.xt = []
        self.yt = []
        self.flag_rotation = 0

        '''
        ros subscribers
        '''
        #订阅自身信息
        rospy.Subscriber('/gs/command', Int32, self.command_callback)
        # rospy.Subscriber('/'+self.vehicle_type+str(self.id)+'/angle', Vector3, self.uav_angle_callback, queue_size=1,callback_args=nm+1)
        rospy.Subscriber('/pos', Vector3, self.uav_pos_callback, queue_size=1)
        rospy.Subscriber('/vel', Twist, self.uav_vel_callback, queue_size=1)
        rospy.Subscriber('/angle', Vector3, self.uav_angle_callback, queue_size=1)
        ''' 
        ros publishers
        '''
        self.command_L1_pub = rospy.Publisher('/command_L1',Int32, queue_size=1)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.cmd_stage_pub = rospy.Publisher('/cmd_stage', Int32, queue_size=10)

        self.state = 0
        self.start_time = rospy.Time.now()         
        
        print("Initialized")

    """
    Callback functions for subscribers.
    """  
    def uav_angle_callback(self, msg):
        roll = msg.x /180 *math.pi                              # 滚转角 roll angle(rad) Front
        pitch = -msg.y / 180 * math.pi                          # 俯仰角 pitch angle(rad) Left (negative when rise)
        yaw = -(msg.z / 180 * math.pi- 0.5 * math.pi)          # 偏航角    
        yaw = math.atan2(math.sin(yaw), math.cos(yaw)) 
        self.uav_angle[0] = roll
        self.uav_angle[1] = pitch
        self.uav_angle[2] = yaw

    def uav_pos_callback(self, msg):
        # print(msg)
        self.uav_pos[0] = msg.y
        self.uav_pos[1] = msg.x
        self.uav_pos[2] = msg.z

    def uav_vel_callback(self,msg):
        # print(msg)
        self.uav_vel[1] = msg.linear.x
        self.uav_vel[0] = msg.linear.y
        self.uav_vel[2] = msg.linear.z
        self.v = math.sqrt(msg.linear.x**2 + msg.linear.y **2+msg.linear.z**2)

    def send_cmd2(self, v, dpsi, h):
        self.roll_cmd = math.atan(-dpsi * v / self.g) * 180 / math.pi  # 偏航角率转滚转
        self.roll_cmd = min(max(self.roll_cmd, -self.roll_thres), self.roll_thres)
        # 发布指令
        cmd_vel = Twist()
        cmd_vel.linear.x = v
        cmd_vel.linear.y = -h
        cmd_vel.linear.z = self.roll_cmd     # 滚转
        cmd_vel.angular.y = 1     # 使能
        # print(11111111111111111111111111111111111111111111111111111111111111111)
        self.cmd_vel_pub.publish(cmd_vel)

    def send_cmd1(self, v, roll, h):
        # 发布指令
        cmd_vel = Twist()
        cmd_vel.linear.x = v
        cmd_vel.linear.y = -h
        cmd_vel.linear.z = roll     # 滚转
        cmd_vel.angular.y = 1     # 使能
        # print(11111)
        self.cmd_vel_pub.publish(cmd_vel)

    def command_callback(self, msg):
        global cnt
        if self.flag_cmd == 0:
            self.flag_cmd = 1
            self.cmd_stage_pub.publish(1)

        if msg.data == 4:
            self.state = 1
            self.scene = msg.data
            # cnt = 0
            print('Receive command  ', msg.data)
            print("开始执行L1制导律")

        # elif msg.data == 3:
        #     self.state = 3
        #     self.scene = msg.data
        #     print('Receive command  ', msg.data)
        #     print("发现目标，停止执行L1制导律")
        else:
            self.state = 0
            self.scene = msg.data
            print('Receive command  ', msg.data)
            print('进入等待状态')
        # elif msg.data == 13:
        #     self.state = 10
        #     print('Receive command  ', msg.data)
        #     print("开始模拟目标运动--3对1！！")
        # elif msg.data == 10:
        #     self.state = 11
        #     print('Receive command  ', msg.data)
        #     print("目标被命中！！")
        # elif msg.data == 999:
        #     self.state = 0
        #     print('Receive command  ', msg.data)
        #     print("停止运动！！")
        

    def control(self):
        # t = rospy.get_time() - self.start_time

        if self.state == 0:         # 初始化
            start_time = rospy.get_time()
            print("等待")

        elif self.state == 1:       # L1航迹跟踪
            global cnt
            # print(self.current_phase)
            if self.v!=0:
                self.L1_control(cnt,self.id,1)#TODO：防碰撞未加
                # print((xt[cnt+1] - self.uav_pos[0])**2+ (self.yt[cnt+1] - self.uav_pos[1])**2)
            if scene_flag == 1:
                r = 200
            else:
                r = 270
            if math.sqrt((self.xt[cnt+1] - self.uav_pos[0])**2+ (self.yt[cnt+1] - self.uav_pos[1])**2)<r:
            # if math.sqrt((self.xt[cnt+1] - self.uav_pos[0])**2+ (self.yt[cnt+1] - self.uav_pos[1])**2)<250:
                cnt=cnt + 1
                if cnt>=self.pn:
                    self.state = 2
                    self.command_L1_pub.publish(2)
                    # self.publish_target_setpoint(velocity_sp=np.array([self.v, self.roll, self.alt]))                
            
            self.send_cmd2(self.v_sp,self.dpsi,self.alt)
            # self.send_cmd1(self.v_sp, self.roll, self.alt)

        elif self.state == 2:
            print('L1 over! ratation again')
            cnt = 0
            self.state = 1

        # elif self.state == 3:
        #     print('stop search')

        
    """
    Functions for control.
    """
        
    def L1_control(self,t,id,print_flag):
        
        global seq_begin,flag
        # local_pose = [0,0,0]
        # local_vels = TwistStamped()

        local_pose = self.uav_pos
        local_vels = self.uav_vel
        local_angles = self.uav_angle
        # print(local_vels)
        uav_v = self.v
        roll_m = 0.6; #最大滚转角度 rad]
        # print(id)
        # local_angles = self.to_euler_angles(local_pose.pose.orientation.w,local_pose.pose.orientation.x,local_pose.pose.orientation.y,local_pose.pose.orientation.z)
        # uav_v = math.pow(local_vels.twist.linear.x ** 2+local_vels.twist.linear.y ** 2+local_vels.twist.linear.z ** 2,1/2)            #无人机空速+local_vel.twist.linear.z ** 2
        # print(id)   
            # print(id)

        self.t_now_loop = rospy.get_time()      #取当前时间
        # print(self.t_now_loop)
        # L1制导程序跟航线
        L1 = 5 * uav_v
        ce0 = [local_pose[0], local_pose[1]]
        r0 = L1
        p1 = [self.xt[t],self.yt[t]]
        dn = [self.xt[t+1] - local_pose[0], self.yt[t+1] - local_pose[1]]
        #if self.t_now_loop - self.t_last_loop >self.print_h:
            # print('目标航点{}: {}'.format(t+2, [self.xt[t+1],self.yt[t+1]]))
            #self.t_last_loop = self.t_now_loop
        fp = [self.xt[t+1],self.yt[t+1]]
        inp = self.line_intersect_circle(ce0, r0, p1, fp)      # L1点
        if inp == []:     #无交点
            p_1 = [local_pose[0], local_pose[1]]
            inp = self.line_intersect_circle(ce0,r0, p_1, fp) 
        d1 = [inp[0][0]- local_pose[0], inp[0][1] - local_pose[1]]   # x,y真实位置 X东，Y北  
        d2 = [self.xt[t+1] - local_pose[0], self.yt[t+1] - local_pose[1]] 
        vv = [local_vels[0], local_vels[1]]        # 真实速度向量   
        if np.dot(d2,d1) < 0:
            d1 = [inp[1][0] - local_pose[0], inp[1][1] - local_pose[1]]
        thta1 = math.atan2(d1[1], d1[0]) 
        thta2 = math.atan2(vv[1], vv[0])
        yta = thta1 - thta2
        # print(L1)
        self.um = (2 * uav_v**2 / L1) * (math.sin(yta))  # L1制导律公式

        self.dpsi = self.um/uav_v
        
        # self.send_cmd2(v = vt, u = self.um, h = self.alt)
        roll_t = math.atan(-self.um/9.80665)
        roll_t = max(min(roll_t,0.8*roll_m),-0.8*roll_m)
        self.roll = 1.0*roll_t
        # self.alt = 100    #30  60
        # print(self.alt)self.self.xt[i] = x_s+width*(i//2)
        if self.flag:
            self.flag = 0
        else:
            self.flag = 1
                
        if self.flag == 1:
            print("--------------------------------------------")
            # print("打击目标的坐标: x = {:7.2f} m, y = {:7.2f} m".format(self.xt[t],self.yt[t]))
            print("当前无人机ID: id ={:7.2f}".format(self.id))
            print("当前无人机坐标: x = {:7.2f} m, y = {:7.2f} m".format(local_pose[0],local_pose[1]))
            print("无人机速度: {:7.2f} m/s".format(self.v))
            print("无人机滚转角: {:7.2f} deg".format(self.roll*57.3))
            print("无人机高度: {:7.2f} m".format(local_pose[2]))
            print('目标航点{}: {}'.format(t+1, [self.xt[t+1],self.yt[t+1]]))
            print("--------------------------------------------")

            # print(theta_t,local_angles[3],d_theta,w_t)
        '''
        roll_t = math.atan(w_t*uav_v/9.80665)
        roll_t = max(min(roll_t,roll_m),-roll_m)
        '''

        # self.v = v_t
        # self.roll = -1*roll_t
        # self.alt = 40
        # seq = local_pose.header.seq*0.02
           
    
    
    def wrap_to_pi(self,theta):
        return (theta + math.pi) % (2 * math.pi) - math.pi

    #防抖
    def sign_replace_function(self,s):
        # eplision = 0.1
        # return s/(abs(s)+eplision)
        eplision = 0.5
        if abs(s/eplision)<=1:
            temp = s/eplision
        else:
            temp = self.sign(s)
        
        return temp

    def sign(self,x):
        if x>0:
            return 1
        elif x==0:
            return 0
        else:
            return -1
    
    # 计算圆 与 线段相交的点
    def line_intersect_circle(self,circle_center,r,point1,point2):
        x0, y0 = circle_center
        x1, y1 = point1[0:2]
        x2, y2 = point2[0:2]
        if x1 == x2:
            inp = []
            if abs(r) >= abs(x1 - x0):
                #下方这个点
                p1 = x1, round(y0 - (r ** 2 - (x1 - x0) ** 2)**(0.5), 5)
                #上方这个点
                p2 = x1, round(y0 +(r ** 2 - (x1 - x0) ** 2)**(0.5), 5)
                inp.append(p2)
                inp.append(p1)

        else:
            #求直线y=kx+b的斜率及b
            k = (y1 - y2) / (x1 - x2)
            b0 = y1 - k * x1
            #直线与圆的方程化简为一元二次方程ax**2+bx+c=0
            a = k ** 2 + 1
            b = 2 * k * (b0 - y0) - 2 * x0
            c = (b0 - y0) ** 2 + x0 ** 2 - r ** 2
            #判别式判断解
            delta = b ** 2 - 4 * a * c
            if delta >= 0:
                p1x = round((-b - delta**(0.5)) / (2 * a), 5)
                p2x = round((-b + delta**(0.5)) / (2 * a), 5)
                p1y = round(k * p1x + b0, 5)
                p2y = round(k * p2x + b0, 5)
                inp = [[p1x, p1y], [p2x, p2y]]
            else:
                inp = []
        return inp       
    
    def Vector3_pack(self, x, y, z):
        # 封装Vector3数据
        data =  Vector3()
        data.x = x
        data.y = y
        data.z = z
        return data
    
    def to_euler_angles(self,w=0, x=0, y=0, z=0):#返回值为弧度制
        angles = {}
        r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
        p = math.asin(2*(w*y-z*x))
        y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))

        angles[1] = r#*180/math.pi
        angles[2] = p#*180/math.pi
        angles[3] = y#*180/math.pi
        #print (r,p,y)
        return angles
    
    def gps2xy(self, lat, lon, ref_lat, ref_lon):
        # input GPS and Reference GPS in degrees
        # output XY in meters (m) X:North Y:East
        CONSTANTS_RADIUS_OF_EARTH = 6371000. # (m)
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        sin_lat = math.sin(lat_rad)
        cos_lat = math.cos(lat_rad)
        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        cos_d_lon = math.cos(lon_rad - ref_lon_rad)

        arg = np.clip(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0)
        c = math.acos(arg)

        k = 1.0
        if abs(c) > 0:
            k = (c / math.sin(c))

        x = float(k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH)
        y = float(k * cos_lat * math.sin(lon_rad - ref_lon_rad) * CONSTANTS_RADIUS_OF_EARTH)

        return y, x
    
    # gps点坐标转换为新gps原点坐标系下的xy坐标，两次gps2xy转换
    def gps_to_xy(self, lat, lon, ref_lat, ref_lon):
        # 原点偏移量
        offset_y, offset_x = self.gps2xy(self.begin_lat, self.begin_lon, self.px4_origin_lat, self.px4_origin_lon)
        # 当前点偏移量
        current_y, current_x = self.gps2xy(lat, lon, ref_lat, rXFD/src/xgc-fixed-wing/src/ef_lon)
        # 转换后的xy坐标
        new_x = current_x + offset_x
        new_y = current_y + offset_y
        return new_y, new_x
        

    # from geographiclib.geocentric import Geocentric
    def ECEF_to_LLA(self, x, y, z):
        geocentric = Geocentric.WGS84()
        lat, lon, alt = geocentric.Reverse(x, y, z)
        return lat, lon, alt
        # self.px4_origin_lat, self.px4_origin_lon, self.px4_origin_alt = self.ECEF_to_LLA(0, 0, 0) 通过gp_origin话题获取ECEF坐标系原点



    """
    Main loop.
    """
    def run(self):
        
        if scene_flag == 1:
            # 按照给定航点运行
            self.xt = xl
            self.yt = yl
            self.pn = 8
        
        else:
            # # 按照边界点生成航点方法
            # x_max = max(xl)
            # x_min = min(xl)
            # y_max = max(yl)
            # y_min = min(yl)
            # # 可考虑扩大搜索范围

            # x_s = (x_max-x_min)/4*(self.id-1)+x_min
            # x_e = (x_max-x_min)/4*self.id+x_min
                
            p1=[xl[0],yl[0]]
            p2=[xl[1],yl[1]]
            p3=[xl[2],yl[2]]
            p4=[xl[3],yl[3]]
            length_r = math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
            width_r = math.sqrt((p3[0]-p2[0])**2+(p3[1]-p2[1])**2)

            # # 搜索方法一：搜索时间过长，可能有遗漏
            # if x_e-x_s > 4*radius + 2*width:
            #     loop_num = math.ceil((x_e-x_s)/(2*width))
            #     self.flag_rotation = 1
            # else:
            #     loop_num = math.ceil((x_e-x_s)/width)
            #     self.flag_rotation = 2

            # self.xt=[0 for i in range(2*loop_num+1)]
            # self.yt=[0 for i in range(2*loop_num+1)]
            # self.pn = 2*loop_num

            # if self.flag_rotation == 1:
            #     for i in range(1,2*loop_num+1):
            #         if i%4 == 1:
            #             self.xt[i] = x_s+width*((i-1)//4)
            #             self.yt[i] = y_max
            #         elif i%4 == 2:
            #             self.xt[i] = x_s+width*((i-1)//4)
            #             self.yt[i] = y_min
            #         elif i%4 == 3:
            #             self.xt[i] = x_s+width*((i-1)//4)+(x_e-x_s)/2
            #             self.yt[i] = y_min
            #         else:
            #             self.xt[i] = x_s+width*((i-1)//4)+(x_e-x_s)/2
            #             self.yt[i] = y_max
            #     print('id:',self.id,self.xt,self.yt)
                
            # elif self.flag_rotation == 2:
            #     for i in range(1,2*loop_num+1):
            #         if i%4 == 1:
            #             self.xt[i] = x_s+width*((i-1)//4)
            #             self.yt[i] = y_max
            #         elif i%4 == 2:
            #             self.xt[i] = x_s+width*((i-1)//4)
            #             self.yt[i] = y_min
            #         elif i%4 == 3:
            #             self.xt[i] = x_s+width*((i-1)//4)+2*radius+width
            #             self.yt[i] = y_min
            #         else:
            #             self.xt[i] = x_s+width*((i-1)//4)+2*radius+width
            #             self.yt[i] = y_max
            #     print('id:',self.id,self.xt,self.yt)

            # # 搜索方法二:短边地毯式方形搜索
            # # loop_num = math.ceil((y_max-y_min)/(width))
            # loop_num = math.ceil(width_r/width)
            # print(loop_num)

            # s_s = [(p2[0]-p1[0])/4*(self.id-1)+p1[0],(p2[1]-p1[1])/4*(self.id-1)+p1[1]]
            # s_e = [(p2[0]-p1[0])/4*self.id+p1[0],(p2[1]-p1[1])/4*self.id+p1[1]]

            # self.xt=[0 for i in range(2*loop_num+1)]
            # self.yt=[0 for i in range(2*loop_num+1)]
            # self.pn = 2*loop_num

            # for i in range(1,2*loop_num+1):
            #     if i%4 == 1:
            #         self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*width*((i-1)//2+1/2)
            #         # self.yt[i] = y_min+width*((i-1)//2)+width/2
            #         self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*width*((i-1)//2+1/2)
            #     elif i%4 == 2:
            #         self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*width*((i-1)//2+1/2)
            #         # self.yt[i] = y_min+width*((i-1)//2)+width/2
            #         self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*width*((i-1)//2+1/2)
            #     elif i%4 == 3:
            #         self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*width*((i-1)//2+1/2)
            #         # self.yt[i] = y_min+width*((i-1)//2)+width/2
            #         self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*width*((i-1)//2+1/2)
            #     else:
            #         self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*width*((i-1)//2+1/2)
            #         # self.yt[i] = y_min+width*((i-1)//2)+width/2
            #         self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*width*((i-1)//2+1/2)
            # print('id:',self.id,self.xt,self.yt)

            # # 搜索方法三:短边三角形搜索
            # loop_num = math.ceil((y_max-y_min)/(width))
            # print(loop_num)

            # self.xt=[0 for i in range(2*loop_num+1)]
            # self.yt=[0 for i in range(2*loop_num+1)]
            # self.pn = 2*loop_num

            # for i in range(1,2*loop_num+1):
            #     if i%2 == 1:
            #         self.xt[i] = x_s
            #         self.yt[i] = y_min+width*((i-1)//2)
            #     else:
            #         self.xt[i] = x_e
            #         self.yt[i] = y_min+width*((i-1)//2)+width/2
            # print('id:',self.id,self.xt,self.yt)

            # 搜索方法四：两机协同短边搜索
            width_init = [350,1150] # 一号飞机所在位置,二号飞机所在位置
            loop_num = math.ceil(width_r/width)
            print(loop_num)

            s_s = [(p2[0]-p1[0])/2*((self.id-1)//2)+p1[0],(p2[1]-p1[1])/2*((self.id-1)//2)+p1[1]]
            s_e = [(p2[0]-p1[0])/2*((self.id-1)//2+1)+p1[0],(p2[1]-p1[1])/2*((self.id-1)//2+1)+p1[1]]

            self.xt=[0 for i in range(2*loop_num+1)]
            self.yt=[0 for i in range(2*loop_num+1)]
            self.pn = 2*loop_num
            
            if (self.id-1)//2 == 0:
                for i in range(1,2*loop_num+1):
                    if i%4 == 1:
                        self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                        self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                    elif i%4 == 2:
                        self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                        self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                    elif i%4 == 3:
                        self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                        self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                    else:
                        self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-1])
                        self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-1])
            else:
                for i in range(1,2*loop_num+1):
                    if i%4 == 1:
                        self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                        self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                    elif i%4 == 2:
                        self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                        self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                    elif i%4 == 3:
                        self.xt[i] = s_e[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                        self.yt[i] = s_e[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                    else:
                        self.xt[i] = s_s[0]+(p3[0]-p2[0])/width_r*(width*((i-1)//2)+width_init[self.id-3])
                        self.yt[i] = s_s[1]+(p3[1]-p2[1])/width_r*(width*((i-1)//2)+width_init[self.id-3])
            print('id:',self.id,self.xt,self.yt)

        while not rospy.is_shutdown():
            self.control()

            # Publish own state for other drones
            # self.state_pub.publish(self.local_position)
            
            rate.sleep()

if __name__ == '__main__':
    try:
        cnt = 0
        # uav_id = int(sys.argv[1]) #这里用的是 1 2 3 4
        # width = 150 #用于方法一，画口字形
        # width = 700 #用于方法二、地毯式
        width = 800 #用于方法四、双机协同
        radius = 400 
        uav_id = rospy.get_param('uav_id', 1)
        rospy.init_node('threeDL1_tracking' + str(uav_id))
        rate = rospy.Rate(50)
        scene_flag = 0 #涿州场地是1 海上场景是0
        
        if scene_flag == 1:
            # 按照给定航点飞行
            alt = [200,250,300,350]
            speed = 20
            pn = 8  # 设置航点数量 与 FP 对应的索引刚好对应上！
            xl = [0 for i in range(pn+1)]
            yl = [0 for i in range(pn+1)]
            folder = os.path.dirname(os.path.abspath(__file__))
            path = folder + '/guidance_tar_temp.xlsx' # 给定所有航点a
            wb = openpyxl.load_workbook(path, read_only=True, data_only=True)
            ws = wb['Sheet1']
            for i in range(1,9):
                xl[i] = ws.cell(i,1).value
                yl[i] = ws.cell(i,2).value
        
        else:
            # 按照边界点生成航点
            # alt = [200,210,220,230] # 郑州泡沫机
            alt = [400,410,420,430] # 海上XFD
            speed = 35 # 海上XFD
            # speed = 24 # 郑州泡沫机
            # xl = [0 for i in range(4)]
            # yl = [0 for i in range(4)]
            # folder = os.path.dirname(os.path.abspath(__file__))
            # path = folder + '/guidance_boundary.xlsx' # 给定边界点航点自动生成
            # wb = openpyxl.load_workbook(path, read_only=True, data_only=True)
            # ws = wb['Sheet1']
            # for i in range(1,5):
            #     xl[i-1] = ws.cell(i,1).value
            #     yl[i-1] = ws.cell(i,2).value

            # 海上坐标参数
            # xl = [0, 14816, 14816, 0]
            # yl = [0, 0, -14816, -14816]
            xl = [0, 0, 13200, 13200]
            yl = [0, -14816, -14816, 0]
            # # 郑州坐标参数 原先整个场地范围
            # xl = [-1698, 5230, 1603, -4906]
            # yl = [-5357, 3066, 5812, -2512] 
            # # 郑州坐标参数 缩比范围
            # xl = [34, 3498, -24, -3278]
            # yl = [-3251, 960, 3731, -431] 

        control_node = L1()
        control_node.run()
    except rospy.ROSInterruptException as e:
        print(e)
    


